/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/common/time/time.h"
#include <chrono>

#include "cyber/common/file.h"
#include "cyber/common/log.h"

#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"

// routing模块的头文件
#include "modules/routing/routing_component.h"
#include "modules/routing/common/routing_gflags.h"
#include "modules/routing/routing.h"
#include "modules/routing/proto/routing.pb.h"

// planning模块的头文件
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/integration_tests/planning_test_base.h"

// control模块的头文件
#include "modules/control/control_component.h"
#include "modules/control/common/control_gflags.h"
#include "modules/control/proto/control_conf.pb.h"
#include "modules/control/proto/control_cmd.pb.h"

#include "include/global_defination.h" // 定义了 WORK_SPACE_PATH

// routing模块
bool routing_test()
{

    // Load the routing request file, routing request waypoints
    std::string routing_test_waypoints = WORK_SPACE_PATH + "/modules/routing_pnc_test/test_data/routing_test_waypoints.txt";
    apollo::routing::RoutingRequest routing_request;
    if (!apollo::cyber::common::GetProtoFromFile(routing_test_waypoints, &routing_request))
    {
        AERROR << "failed to load file: " << routing_test_waypoints;
        return false;
    }
    AERROR << "&routing_request   " << &routing_request;

    apollo::routing::RoutingRequest *routing_request_1 = new apollo::routing::RoutingRequest(routing_request);
    std::shared_ptr<apollo::routing::RoutingRequest> routing_request_2(routing_request_1);
    // generate topo_graph   // modules/common/data/global_flagfile.txt
    apollo::routing::Routing routing_;
    routing_.Init();
    routing_.Start();

    // routing process
    auto response = std::make_shared<apollo::routing::RoutingResponse>();
    routing_.Process(routing_request_2, response.get());
    // 打印routing的结果
    AERROR << "routing results:     " << (response->status().msg());
    response->road().size();

    for (int i = 0; i < response->road().size(); i++)
    {
        std::cout << "  " << response->road()[i].id() << std::endl;
    }

    response->measurement();
    response->routing_request();
    response->map_version();
    response->status();
}
// routing模块,结束

// planning模块
namespace apollo
{
    namespace planning
    {
        // 绝对路径
        // /home/test_algorithm/algorithm/apollo_cmake_raw_version/src/apollo
        std::string pre_path_base = WORK_SPACE_PATH;

        using apollo::canbus::Chassis;
        using apollo::common::time::Clock;
        using apollo::localization::LocalizationEstimate;
        using apollo::perception::TrafficLightDetection;
        using apollo::prediction::PredictionObstacles;
        using apollo::routing::RoutingResponse;

        // PlanningBase指针
        std::unique_ptr<PlanningBase> planning_ = nullptr;
        std::map<TrafficRuleConfig::RuleId, bool> rule_enabled_;
        // 规划出的轨迹点
        ADCTrajectory adc_trajectory_;
        // planning模块的输入: localization,routing,obstacle,chassis
        LocalView local_view_;
        // planning模块的配置参数
        PlanningConfig config_;

        // 文件路径地址的前缀
        DEFINE_string(pre_dir, "", "the test data folder");
        // 是否写进文件
        DEFINE_bool(write_into_file, false, "true to update decision golden log file.");
        // routing_msg文件的地址
        DEFINE_string(test_routing_response_file, "", "The routing file used in test");
        // localization_msg文件的地址
        DEFINE_string(test_localization_file, "", "The localization test file");
        // chassis_msg文件的地址
        DEFINE_string(test_chassis_file, "", "The chassis test file");
        // prediction_msg文件的地址
        DEFINE_string(test_prediction_file, "", "The prediction module test file");
        // traffic_light_msg文件的地址
        DEFINE_string(test_traffic_light_file, "", "The traffic light test file");
        // relative_map_msg文件的地址
        DEFINE_string(test_relative_map_file, "", "The relative map test file");
        // 上一帧planning规划出的路径点的文件的地址
        DEFINE_string(test_previous_planning_file, "",
                      "The previous planning test file");

        void Set_file_names()
        {
            FLAGS_use_multi_thread_to_add_obstacles = false;
            FLAGS_enable_multi_thread_in_dp_st_graph = false;
            // planning模块的配置文件
            FLAGS_planning_config_file = pre_path_base +
                                         "/modules/planning/conf/planning_config.pb.txt";
            // traffic_rule的配置文件
            FLAGS_traffic_rule_config_filename = pre_path_base +
                                                 "/modules/planning/conf/traffic_rule_config.pb.txt";
            // 平滑器的配合文件
            FLAGS_smoother_config_filename = pre_path_base +
                                             "/modules/planning/conf/qp_spline_smoother_config.pb.txt";

            // planning模块相关的参数设置
            FLAGS_align_prediction_time = false;
            FLAGS_estimate_current_vehicle_state = false;
            FLAGS_enable_reference_line_provider_thread = false;
            // FLAGS_enable_trajectory_check is temporarily disabled, otherwise EMPlanner
            // and LatticePlanner can't pass the unit test.
            FLAGS_enable_trajectory_check = false;
            FLAGS_planning_test_mode = true;
            FLAGS_enable_lag_prediction = false;
            FLAGS_use_osqp_optimizer_for_reference_line = false;
            FLAGS_use_navigation_mode = false;
            FLAGS_planning_upper_speed_limit = 12.5;
            FLAGS_enable_scenario_stop_sign = false;
            FLAGS_enable_scenario_traffic_light = false;
            FLAGS_enable_rss_info = false;
            // Do not use fallback trajectory during testing
            FLAGS_use_planning_fallback = false;

            // traffic_rule的设置
            rule_enabled_[TrafficRuleConfig::CROSSWALK] = false;

            // 全局变量,载入的地图  // apollo/modules/map/hdmap/hdmap_util.cc 载入地图
            FLAGS_map_dir = pre_path_base + "/modules/routing_pnc_test/test_data/sunnyvale_loop_map";
            FLAGS_test_base_map_filename = "base_map_test.bin";
            FLAGS_base_map_filename = "base_map_test.bin";
            FLAGS_routing_map_filename = "routing_map.bin";
            FLAGS_sim_map_filename = "sim_map.bin";

            // 各个模块产生的msg文件路径的前缀名称
            // 要读入的文件  // routing_msg, prediction_msg, localization_msg, chassis_msg 的文件名称
            FLAGS_pre_dir = pre_path_base + "/modules/routing_pnc_test/test_data/sunnyvale_loop_test";
            std::string seq_num = "1";
            FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
            FLAGS_test_prediction_file = seq_num + "_prediction.pb.txt";
            FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
            FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
            FLAGS_test_traffic_light_file = " ";
        }

        bool FeedTestData()
        {
            // 从文件中读入chassis
            Chassis chassis;
            if (FLAGS_test_chassis_file.empty())
            {
                AERROR << "Requires FLAGS_test_chassis_file to be set";
                return false;
            }
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_chassis_file, &chassis))
            {
                AERROR << "failed to load file: " << FLAGS_test_chassis_file;
                AERROR << (FLAGS_pre_dir + "/" + FLAGS_test_chassis_file);
                return false;
            }

            // 从文件中读入localization
            if (FLAGS_test_localization_file.empty())
            {
                AERROR << "Requires FLAGS_test_localization_file to be set";
                return false;
            }
            LocalizationEstimate localization;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_localization_file,
                    &localization))
            {
                AERROR << "failed to load file: " << FLAGS_test_localization_file;
                return false;
            }
            Clock::SetMode(Clock::MOCK);
            Clock::SetNowInSeconds(localization.header().timestamp_sec());

            // 从文件中读入prediction
            if (FLAGS_test_prediction_file.empty())
            {
                AERROR << "Requires FLAGS_test_prediction_file to be set";
                return false;
            }
            PredictionObstacles prediction;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_prediction_file,
                    &prediction))
            {
                AERROR << "failed to load file: " << FLAGS_test_prediction_file;
                return false;
            }

            // 从文件中读入routing_response
            if (FLAGS_test_routing_response_file.empty())
            {
                AERROR << "Requires FLAGS_test_routing_response_file";
                return false;
            }
            RoutingResponse routing_response;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_routing_response_file,
                    &routing_response))
            {
                AERROR << "failed to load file: " << FLAGS_test_routing_response_file;
                return false;
            }

            // // 从文件中读入traffic_light_detection  // optional
            // if (FLAGS_test_traffic_light_file.empty())
            // {
            //     AERROR << "Requires FLAGS_test_traffic_light_file";
            //     return false;
            // }
            // TrafficLightDetection traffic_light_detection;
            // if (!apollo::cyber::common::GetProtoFromFile(
            //         FLAGS_pre_dir + "/" + FLAGS_test_traffic_light_file,
            //         &traffic_light_detection))
            // {
            //     AERROR << "failed to load file: " << FLAGS_test_traffic_light_file;
            //     return false;
            // }

            // 组帧为local_view_
            local_view_.prediction_obstacles =
                std::make_shared<PredictionObstacles>(prediction);
            local_view_.chassis = std::make_shared<Chassis>(chassis);
            local_view_.localization_estimate =
                std::make_shared<LocalizationEstimate>(localization);
            local_view_.routing =
                std::make_shared<routing::RoutingResponse>(routing_response);
            // local_view_.traffic_light =
            //     std::make_shared<TrafficLightDetection>(traffic_light_detection);

            AINFO << "Successfully feed proto files.";
            return true;
        }

        void SetUp()
        {
            Set_file_names();

            if (FLAGS_use_navigation_mode)
            {
                // TODO(all)
                // planning_ = std::unique_ptr<PlanningBase>(new NaviPlanning());
            }
            else
            {
                planning_ = std::unique_ptr<PlanningBase>(new OnLanePlanning());
            }

            // 从文件里面读入其他模块的数据
            CHECK(FeedTestData()) << "Failed to feed test data";
            // planning模块的配置文件
            CHECK(cyber::common::GetProtoFromFile(FLAGS_planning_config_file, &config_))
                << "failed to load planning config file " << FLAGS_planning_config_file;
            // 初始化PlanningBase指针
            CHECK(planning_->Init(config_).ok()) << "Failed to init planning module";

            // 如果存在上一帧planning规划出的路径点
            if (!FLAGS_test_previous_planning_file.empty())
            {
                const auto prev_planning_file =
                    FLAGS_pre_dir + "/" + FLAGS_test_previous_planning_file;
                ADCTrajectory prev_planning;
                CHECK(cyber::common::GetProtoFromFile(prev_planning_file, &prev_planning));
                planning_->last_publishable_trajectory_.reset(
                    new PublishableTrajectory(prev_planning));
            }

            // 交通规则的设置
            for (auto &config : *(planning_->traffic_rule_configs_.mutable_config()))
            {
                auto iter = rule_enabled_.find(config.rule_id());
                if (iter != rule_enabled_.end())
                {
                    config.set_enabled(iter->second);
                }
            }
        }

        void UpdateData()
        {
            CHECK(FeedTestData()) << "Failed to feed test data";

            if (!FLAGS_test_previous_planning_file.empty())
            {
                const auto prev_planning_file =
                    FLAGS_pre_dir + "/" + FLAGS_test_previous_planning_file;
                ADCTrajectory prev_planning;
                CHECK(cyber::common::GetProtoFromFile(prev_planning_file, &prev_planning));
                planning_->last_publishable_trajectory_.reset(
                    new PublishableTrajectory(prev_planning));
            }
            for (auto &config : *planning_->traffic_rule_configs_.mutable_config())
            {
                auto iter = rule_enabled_.find(config.rule_id());
                if (iter != rule_enabled_.end())
                {
                    config.set_enabled(iter->second);
                }
            }
        }

        void TrimPlanning(ADCTrajectory *origin,
                          bool no_trajectory_point)
        {
            origin->clear_latency_stats();
            origin->clear_debug();
            // origin->mutable_header()->clear_radar_timestamp();
            // origin->mutable_header()->clear_lidar_timestamp();
            // origin->mutable_header()->clear_timestamp_sec();
            // origin->mutable_header()->clear_camera_timestamp();
            // origin->mutable_header()->clear_sequence_num();

            if (no_trajectory_point)
            {
                origin->clear_total_path_length();
                origin->clear_total_path_time();
                origin->clear_trajectory_point();
            }
        }

        bool IsValidTrajectory(const ADCTrajectory &trajectory)
        {
            for (int i = 0; i < trajectory.trajectory_point_size(); ++i)
            {
                const auto &point = trajectory.trajectory_point(i);

                const double kMaxAccelThreshold =
                    FLAGS_longitudinal_acceleration_upper_bound;
                const double kMinAccelThreshold =
                    FLAGS_longitudinal_acceleration_lower_bound;
                if (point.a() > kMaxAccelThreshold || point.a() < kMinAccelThreshold)
                {
                    AERROR << "Invalid trajectory point because accel out of range: "
                           << point.DebugString();
                    return false;
                }

                if (!point.has_path_point())
                {
                    AERROR << "Invalid trajectory point because NO path_point in "
                              "trajectory_point: "
                           << point.DebugString();
                    return false;
                }

                if (i > 0)
                {
                    const double kPathSEpsilon = 1e-3;
                    const auto &last_point = trajectory.trajectory_point(i - 1);
                    if (point.path_point().s() + kPathSEpsilon <
                        last_point.path_point().s())
                    {
                        AERROR << "Invalid trajectory point because s value error. last point: "
                               << last_point.DebugString()
                               << ", curr point: " << point.DebugString();
                        return false;
                    }
                }
            }
            return true;
        }

        bool RunPlanning(const std::string &test_case_name,
                         int case_num, bool no_trajectory_point)
        {
            const std::string golden_result_file = apollo::common::util::StrCat(
                "result_", test_case_name, "_", case_num, ".pb.txt");

            std::string full_golden_path = FLAGS_pre_dir + "/" + golden_result_file;

            ADCTrajectory adc_trajectory_pb;
            planning_->RunOnce(local_view_, &adc_trajectory_pb);
            printf("planning_->RunOnce   \n");

            if (!IsValidTrajectory(adc_trajectory_pb))
            {
                AERROR << "Fail to pass trajectory check.";
                return false;
            }

            adc_trajectory_ = adc_trajectory_pb;
            TrimPlanning(&adc_trajectory_, no_trajectory_point);
            // 规划出的路径点
            ADEBUG << adc_trajectory_.ShortDebugString();

            if (FLAGS_write_into_file)
                if (1)
                {
                    AINFO << "The golden file is regenerated:" << full_golden_path;
                    cyber::common::SetProtoToASCIIFile(adc_trajectory_, full_golden_path);
                }
                else
                {
                    ADCTrajectory golden_result;
                    bool load_success =
                        cyber::common::GetProtoFromASCIIFile(full_golden_path, &golden_result);
                    TrimPlanning(&golden_result, no_trajectory_point);
                    if (!load_success ||
                        !common::util::IsProtoEqual(golden_result, adc_trajectory_))
                    {
                        char tmp_fname[100] = "/tmp/XXXXXX";
                        int fd = mkstemp(tmp_fname);
                        if (fd < 0)
                        {
                            AERROR << "Failed to create temporary file: " << tmp_fname;
                            return false;
                        }
                        if (!cyber::common::SetProtoToASCIIFile(adc_trajectory_, fd))
                        {
                            AERROR << "Failed to write to file: " << tmp_fname;
                        }
                        AERROR << "found error\ndiff -y " << tmp_fname << " " << full_golden_path;
                        AERROR << "to override error\nmv " << tmp_fname << " "
                               << full_golden_path;
                        AERROR << "to visualize\n/usr/bin/python "
                                  "modules/tools/plot_trace/plot_planning_result.py "
                               << tmp_fname << " " << full_golden_path;
                        return false;
                    }
                }
            return true;
        }

        TrafficRuleConfig *GetTrafficRuleConfig(
            const TrafficRuleConfig::RuleId &rule_id)
        {
            for (auto &config : *planning_->traffic_rule_configs_.mutable_config())
            {
                if (config.rule_id() == rule_id)
                {
                    return &config;
                }
            }
            return nullptr;
        }

    } // namespace planning
} // namespace apollo
// planning模块,结束

// control模块
namespace apollo
{
    namespace control
    {
        using apollo::canbus::Chassis;
        using apollo::common::monitor::MonitorMessage;
        using apollo::common::time::Clock;
        using apollo::localization::LocalizationEstimate;
        using apollo::planning::ADCTrajectory;

        // DECLARE_string(control_test_localization_file);
        // DECLARE_string(control_test_pad_file);
        // DECLARE_string(control_test_planning_file);
        // DECLARE_string(control_test_chassis_file);
        // DECLARE_string(control_test_data_dir);
        // DECLARE_string(control_test_monitor_file);

        DEFINE_string(control_test_chassis_file, "", "chassis input file");
        DEFINE_string(control_test_data_dir, "", "the test data folder");
        DEFINE_string(control_test_localization_file, "", "localization input file");
        DEFINE_string(control_test_monitor_file, "", "montor input file");
        DEFINE_string(control_test_pad_file, "", "pad message input file");
        DEFINE_string(control_test_planning_file, "", "planning input file");
        DEFINE_bool(control_test_update_golden_log, false, "true to update golden log file.");

        ControlCommand control_command_;
        ControlComponent control_;
        uint32_t s_seq_num_ = 0;

        // 设置配置文件的路径
        void SetUp()
        {
            FLAGS_enable_csv_debug = true;
            FLAGS_is_control_test_mode = true;
            FLAGS_control_conf_file = WORK_SPACE_PATH + "/modules/routing_pnc_test/test_data/simple_control_test/control_conf.pb.txt";
            FLAGS_control_test_data_dir = WORK_SPACE_PATH + "/modules/routing_pnc_test/test_data/simple_control_test/";

            FLAGS_control_test_localization_file = "1_localization.pb.txt";
            FLAGS_control_test_pad_file = "1_pad.pb.txt";
            FLAGS_control_test_planning_file = "1_planning.pb.txt";
            FLAGS_control_test_chassis_file = "1_chassis.pb.txt";

            // 读入control模块的config文件,初始化controller
            if (!cyber::common::GetProtoFromFile(FLAGS_control_conf_file, &control_.control_conf_))
            {
                AERROR << "Unable to load control conf file: " << FLAGS_control_conf_file;
                exit(EXIT_FAILURE);
            }
            AINFO << "Conf file: " << FLAGS_control_conf_file << " is loaded.";
            // 初始化controller
            if (!control_.controller_agent_.Init(&(control_.control_conf_)).ok())
            {
                AERROR << "Control init controller failed! Stopping...";
                exit(EXIT_FAILURE);
            }
            control_.controller_agent_.Reset();
        }

        void trim_control_command(ControlCommand *origin)
        {
            origin->mutable_header()->clear_radar_timestamp();
            origin->mutable_header()->clear_lidar_timestamp();
            origin->mutable_header()->clear_timestamp_sec();
            origin->mutable_header()->clear_camera_timestamp();
        }

        bool test_control()
        {
            // // 读入control模块的config文件
            // if (!cyber::common::GetProtoFromFile(FLAGS_control_conf_file,
            //                                      &control_.control_conf_))
            // {
            //     AERROR << "Unable to load control conf file: " << FLAGS_control_conf_file;
            //     exit(EXIT_FAILURE);
            // }
            // AINFO << "Conf file: " << FLAGS_control_conf_file << " is loaded.";

            // // 初始化controller
            // if (!control_.controller_agent_.Init(&(control_.control_conf_)).ok())
            // {
            //     AERROR << "Control init controller failed! Stopping...";
            //     exit(EXIT_FAILURE);
            // }
            // control_.controller_agent_.Reset();

            // 读入Pad message
            if (!FLAGS_control_test_pad_file.empty())
            {
                PadMessage pad_message;
                // 修改  FLAGS_control_test_data_dir
                FLAGS_control_test_data_dir = WORK_SPACE_PATH + "/modules/control/testdata/simple_control_test/";
                // 模式设置: stop,start,reset
                if (!cyber::common::GetProtoFromFile(
                        FLAGS_control_test_data_dir + FLAGS_control_test_pad_file, &pad_message))
                {
                    AERROR << "Failed to load PadMesssage from file " << FLAGS_control_test_data_dir;

                    AERROR << "Failed to load PadMesssage from file " << FLAGS_control_test_pad_file;
                    return false;
                }
                control_.OnPad(std::make_shared<apollo::control::PadMessage>(pad_message));
            }

            // 读入Localization
            if (!FLAGS_control_test_localization_file.empty())
            {
                LocalizationEstimate localization;
                if (!cyber::common::GetProtoFromFile(
                        FLAGS_control_test_data_dir + FLAGS_control_test_localization_file,
                        &localization))
                {
                    AERROR << "Failed to load localization file " << FLAGS_control_test_data_dir
                           << FLAGS_control_test_localization_file;
                    return false;
                }
                // 接收localization的消息
                control_.OnLocalization(
                    std::make_shared<apollo::localization::LocalizationEstimate>(
                        localization));
            }

            // 读入Planning
            if (!FLAGS_control_test_planning_file.empty())
            {
                ADCTrajectory trajectory;
                if (!cyber::common::GetProtoFromFile(
                        FLAGS_control_test_data_dir + FLAGS_control_test_planning_file, &trajectory))
                {
                    AERROR << "Failed to load planning file " << FLAGS_control_test_data_dir
                           << FLAGS_control_test_planning_file;
                    return false;
                }
                // 接受planning的消息
                control_.OnPlanning(
                    std::make_shared<apollo::planning::ADCTrajectory>(trajectory));
            }

            // 读入Chassis
            if (!FLAGS_control_test_chassis_file.empty())
            {
                Chassis chassis;
                if (!cyber::common::GetProtoFromFile(
                        FLAGS_control_test_data_dir + FLAGS_control_test_chassis_file, &chassis))
                {
                    AERROR << "Failed to load chassis file " << FLAGS_control_test_data_dir
                           << FLAGS_control_test_chassis_file;
                    return false;
                }
                // 接受chassis的消息
                control_.OnChassis(std::make_shared<apollo::canbus::Chassis>(chassis));
            }

            // 读入Monitor
            if (!FLAGS_control_test_monitor_file.empty())
            {
                MonitorMessage monitor_message;
                if (!cyber::common::GetProtoFromFile(
                        FLAGS_control_test_data_dir + FLAGS_control_test_monitor_file, &monitor_message))
                {
                    AERROR << "Failed to load monitor file " << FLAGS_control_test_data_dir
                           << FLAGS_control_test_monitor_file;
                    return false;
                }
                // 接受monitor的消息
                control_.OnMonitor(monitor_message);
            }

            // 产生控制指令
            auto err = control_.ProduceControlCommand(&control_command_);
            AERROR << control_command_.ShortDebugString();
            if (!err.ok())
            {
                ADEBUG << "control ProduceControlCommand failed";
                return false;
            }
            return true;
        }

        bool test_control(const std::string &test_case_name, int case_num)
        {

            const std::string golden_result_file = apollo::common::util::StrCat(
                "result_", test_case_name, "_", case_num, ".pb.txt");
            std::string tmp_golden_path = "/tmp/" + golden_result_file;
            std::string full_golden_path = FLAGS_control_test_data_dir + golden_result_file;
            control_command_.Clear();

            if (!test_control())
            {
                AERROR << "test control failed";
                return false;
            }

            trim_control_command(&control_command_);
            if (FLAGS_control_test_update_golden_log)
            {
                AINFO << "The golden file is " << tmp_golden_path << " Remember to:\n"
                      << "mv " << tmp_golden_path << " " << FLAGS_control_test_data_dir << "\n"
                      << "git add " << FLAGS_control_test_data_dir << "/" << golden_result_file;
                cyber::common::SetProtoToASCIIFile(control_command_, golden_result_file);
            }
            else
            {
                ControlCommand golden_result;
                bool load_success =
                    cyber::common::GetProtoFromASCIIFile(full_golden_path, &golden_result);
                if (!load_success)
                {
                    AERROR << "Failed to load golden file: " << full_golden_path;
                    cyber::common::SetProtoToASCIIFile(control_command_, tmp_golden_path);
                    AINFO << "Current result is written to " << tmp_golden_path;
                    return false;
                }
                bool same_result =
                    common::util::IsProtoEqual(golden_result, control_command_);
                if (!same_result)
                {
                    std::string tmp_test_result_file = tmp_golden_path + ".tmp";
                    cyber::common::SetProtoToASCIIFile(control_command_,
                                                       tmp_test_result_file);
                    AERROR << "found diff " << tmp_test_result_file << " "
                           << full_golden_path;
                }
            }
            return true;
        }

    } // namespace control
} // namespace apollo
// control模块,结束

using std::chrono::high_resolution_clock;
using std::chrono::milliseconds;
int main()
{
    // 设置各种文件的路径
    apollo::planning::Set_file_names();

    // routing 模块
    if (0)
    {
        high_resolution_clock::time_point beginTime_routing = high_resolution_clock::now();
        // routing模块
        routing_test();
        high_resolution_clock::time_point endTime_routing = high_resolution_clock::now();
        milliseconds timeInterval_routing = std::chrono::duration_cast<milliseconds>(endTime_routing - beginTime_routing);
        std::cout << "routing used  " << timeInterval_routing.count() << " ms\n";
        sleep(1);
    }

    // planning模块
    if (0)
    {
        apollo::planning::SetUp();

        for (int i = 0; i < 1000; i++)
        {
            // 开始时间
            high_resolution_clock::time_point beginTime = high_resolution_clock::now();

            // 从文件里面读入其他模块的数据
            apollo::planning::FeedTestData();
            // plannng的入口函数
            apollo::planning::RunPlanning("planning_demo", 1, false);

            // 结束时间
            high_resolution_clock::time_point endTime = high_resolution_clock::now();
            milliseconds timeInterval = std::chrono::duration_cast<milliseconds>(endTime - beginTime);
            std::cout << "planning used  " << timeInterval.count() << " ms\n";

            sleep(1);
        }
    }

    // control模块
    if (1)
    {
        // 开始时间
        high_resolution_clock::time_point beginTime_control = high_resolution_clock::now();

        apollo::control::SetUp();
        apollo::control::test_control("control_test", 1);

        // 结束时间
        high_resolution_clock::time_point endTime_control = high_resolution_clock::now();
        milliseconds timeInterval_control = std::chrono::duration_cast<milliseconds>(endTime_control - beginTime_control);
        std::cout << "control used  " << timeInterval_control.count() << " ms\n";
    }

    return 0;
}